/*  电机编号
			头

	 9  5       4 8
		1       0
		  |dog|
		  |dog|
		  |dog|
		3       2
	 11 7       6 10
*/

#include <stdio.h>
#include <math.h>
// #include "MiMotor.h"
#include "dog_solve.h"
struct dog_motor leg_angle[4];
struct dog_foot foot_position[4];
struct dog_motor_ouput leg_angle_ouput[4];
struct dog_pd dog_pd[12];

void dog_solve()
{
#ifdef XIAOMI_DOG
	// 电机 顺时针：正 	逆时针：负

	// 右前腿：************************************************************************************************************************************
	//  计算gama角
	leg_angle[0].sol_gamma = -(acos(leg_length1 / (sqrt(foot_position[0].y * foot_position[0].y + foot_position[0].z * foot_position[0].z))) - atan(foot_position[0].z / foot_position[0].y));
	// 计算a1角
	leg_angle[0].a1 = atan(foot_position[0].x / (foot_position[0].z + leg_length1 * sin(leg_angle[0].sol_gamma)));
	// 计算CE长度
	leg_angle[0].l4 = sqrt(foot_position[0].x * foot_position[0].x + (foot_position[0].y - leg_length1 * cos(leg_angle[0].sol_gamma)) * (foot_position[0].y - leg_length1 * cos(leg_angle[0].sol_gamma)) + (foot_position[0].z + leg_length1 * sin(leg_angle[0].sol_gamma)) * (foot_position[0].z + leg_length1 * sin(leg_angle[0].sol_gamma)));
	// 计算a角
	leg_angle[0].sol_alfa = acos((leg_length2 * leg_length2 + leg_angle[0].l4 * leg_angle[0].l4 - leg_length3 * leg_length3) / (2 * leg_length2 * leg_angle[0].l4)) - leg_angle[0].a1;
	// 计算b角
	leg_angle[0].sol_beta = acos((leg_length2 * leg_length2 + leg_length3 * leg_length3 - leg_angle[0].l4 * leg_angle[0].l4) / (2 * leg_length2 * leg_length3)) - M_PI / 2;
	// 转换成角度制
	//  leg_angle[0].sol_gamma=leg_angle[0].sol_gamma/M_PI*180;
	//  leg_angle[0].sol_alfa=leg_angle[0].sol_alfa/M_PI*180;
	//  leg_angle[0].sol_beta=leg_angle[0].sol_beta/M_PI*180;

	// 左前腿：************************************************************************************************************************************
	//  计算gama角
	leg_angle[1].sol_gamma = (acos(leg_length1 / (sqrt(foot_position[1].y * foot_position[1].y + foot_position[1].z * foot_position[1].z))) - atan(foot_position[1].z / foot_position[1].y));
	// 计算a1角
	leg_angle[1].a1 = atan(foot_position[1].x / (foot_position[1].z + leg_length1 * sin(leg_angle[1].sol_gamma)));
	// 计算CE长度
	leg_angle[1].l4 = sqrt(foot_position[1].x * foot_position[1].x + (foot_position[1].y - leg_length1 * cos(leg_angle[1].sol_gamma)) * (foot_position[1].y - leg_length1 * cos(leg_angle[1].sol_gamma)) + (foot_position[1].z + leg_length1 * sin(leg_angle[1].sol_gamma)) * (foot_position[1].z + leg_length1 * sin(leg_angle[1].sol_gamma)));
	// 计算a角
	leg_angle[1].sol_alfa = (acos((leg_length2 * leg_length2 + leg_angle[1].l4 * leg_angle[1].l4 - leg_length3 * leg_length3) / (2 * leg_length2 * leg_angle[1].l4)) - leg_angle[1].a1);
	// 计算b角
	leg_angle[1].sol_beta = (acos((leg_length2 * leg_length2 + leg_length3 * leg_length3 - leg_angle[1].l4 * leg_angle[1].l4) / (2 * leg_length2 * leg_length3)) - M_PI / 2);
	// //转换成角度制
	// leg_angle[1].sol_gamma=leg_angle[1].sol_gamma/M_PI*180;
	// leg_angle[1].sol_alfa=leg_angle[1].sol_alfa/M_PI*180;
	// leg_angle[1].sol_beta=leg_angle[1].sol_beta/M_PI*180;

	// 左后腿：************************************************************************************************************************************
	//  计算gama角
	leg_angle[2].sol_gamma = (acos(leg_length1 / (sqrt(foot_position[2].y * foot_position[2].y + foot_position[2].z * foot_position[2].z))) - atan(foot_position[2].z / foot_position[2].y));
	// 计算a1角
	leg_angle[2].a1 = atan(foot_position[2].x / (foot_position[2].z + leg_length1 * sin(leg_angle[2].sol_gamma)));
	// 计算CE长度
	leg_angle[2].l4 = sqrt(foot_position[2].x * foot_position[2].x + (foot_position[2].y - leg_length1 * cos(leg_angle[2].sol_gamma)) * (foot_position[2].y - leg_length1 * cos(leg_angle[2].sol_gamma)) + (foot_position[2].z + leg_length1 * sin(leg_angle[2].sol_gamma)) * (foot_position[2].z + leg_length1 * sin(leg_angle[2].sol_gamma)));
	// 计算a角
	leg_angle[2].sol_alfa = (acos((leg_length2 * leg_length2 + leg_angle[2].l4 * leg_angle[2].l4 - leg_length3 * leg_length3) / (2 * leg_length2 * leg_angle[2].l4)) - leg_angle[2].a1);
	// 计算b角
	leg_angle[2].sol_beta = (acos((leg_length2 * leg_length2 + leg_length3 * leg_length3 - leg_angle[2].l4 * leg_angle[2].l4) / (2 * leg_length2 * leg_length3)) - M_PI / 2);
	// 转换成角度制
	//  leg_angle[2].sol_gamma=leg_angle[2].sol_gamma/M_PI*180;
	//  leg_angle[2].sol_alfa=leg_angle[2].sol_alfa/M_PI*180;
	//  leg_angle[2].sol_beta=leg_angle[2].sol_beta/M_PI*180;

	// 右后腿:************************************************************************************************************************************
	//  计算gama角
	leg_angle[3].sol_gamma = -(acos(leg_length1 / (sqrt(foot_position[3].y * foot_position[3].y + foot_position[3].z * foot_position[3].z))) - atan(foot_position[3].z / foot_position[3].y));
	// 计算a1角
	leg_angle[3].a1 = atan(foot_position[3].x / (foot_position[3].z + leg_length1 * sin(leg_angle[3].sol_gamma)));
	// 计算CE长度
	leg_angle[3].l4 = sqrt(foot_position[3].x * foot_position[3].x + (foot_position[3].y - leg_length1 * cos(leg_angle[3].sol_gamma)) * (foot_position[3].y - leg_length1 * cos(leg_angle[3].sol_gamma)) + (foot_position[3].z + leg_length1 * sin(leg_angle[3].sol_gamma)) * (foot_position[3].z + leg_length1 * sin(leg_angle[3].sol_gamma)));
	// 计算a角
	leg_angle[3].sol_alfa = (acos((leg_length2 * leg_length2 + leg_angle[3].l4 * leg_angle[3].l4 - leg_length3 * leg_length3) / (2 * leg_length2 * leg_angle[3].l4)) - leg_angle[3].a1);
	// 计算b角
	leg_angle[3].sol_beta = (acos((leg_length2 * leg_length2 + leg_length3 * leg_length3 - leg_angle[3].l4 * leg_angle[3].l4) / (2 * leg_length2 * leg_length3)) - M_PI / 2);
	// 转换成角度制
	//  leg_angle[3].sol_gamma=leg_angle[3].sol_gamma/M_PI*180;
	//  leg_angle[3].sol_alfa=leg_angle[3].sol_alfa/M_PI*180;
	//  leg_angle[3].sol_beta=leg_angle[3].sol_beta/M_PI*180;
	// int yu[12];
#endif
	// 保护运算
	//  for(int i=1;i<5;i++){
	//  	if(leg_angle[i].sol_alfa>360){
	//  		yu[i]=leg_angle[i].sol_alfa/360;
	//  		leg_angle[i].sol_alfa-=360*yu[i];
	//  	}
	//  	if(leg_angle[i].sol_beta>360){
	//  		yu[i]=leg_angle[i].sol_beta/360;
	//  		leg_angle[i].sol_beta-=360*yu[i];
	//  	}
	//  	if(leg_angle[i].sol_gamma>360){
	//  		yu[i]=leg_angle[i].sol_gamma/360;
	//  		leg_angle[i].sol_gamma-=360*yu[i];
	//  	}
	//  	if(leg_angle[i].sol_alfa<-180){
	//  		leg_angle[i].sol_alfa+=360;
	//  	}
	//  	if(leg_angle[i].sol_beta<-180){
	//  		leg_angle[i].sol_beta+=360;
	//  	}
	//  	if(leg_angle[i].sol_gamma<-180){
	//  		leg_angle[i].sol_gamma+=360;
	//  	}
	//  }


// 电机 顺时针：正 	逆时针：负

	// 0-右前腿：************************************************************************************************************************************
	//  计算gama角
	leg_angle[0].sol_gamma = acos(leg_length1 / (sqrt(foot_position[0].y * foot_position[0].y + foot_position[0].z * foot_position[0].z))) - atan(foot_position[0].z / foot_position[0].y);
	// 计算a1角
	leg_angle[0].a1 = atan(foot_position[0].x / (foot_position[0].z + leg_length1 * sin(leg_angle[0].sol_gamma)));
	// 计算CE长度
	leg_angle[0].l4 = sqrt(foot_position[0].x * foot_position[0].x + (foot_position[0].y - leg_length1 * cos(leg_angle[0].sol_gamma)) * (foot_position[0].y - leg_length1 * cos(leg_angle[0].sol_gamma)) + (foot_position[0].z + leg_length1 * sin(leg_angle[0].sol_gamma)) * (foot_position[0].z + leg_length1 * sin(leg_angle[0].sol_gamma)));
	// 计算a角
	leg_angle[0].sol_alfa = acos((leg_length2 * leg_length2 + leg_angle[0].l4 * leg_angle[0].l4 - leg_length3 * leg_length3) / (2 * leg_length2 * leg_angle[0].l4)) - leg_angle[0].a1;
	// 计算b角
	leg_angle[0].sol_beta = acos((leg_length2 * leg_length2 + leg_length3 * leg_length3 - leg_angle[0].l4 * leg_angle[0].l4) / (2 * leg_length2 * leg_length3)) - M_PI / 2;
	// 转换成角度制
	//  leg_angle[0].sol_gamma=leg_angle[0].sol_gamma/M_PI*180;
	//  leg_angle[0].sol_alfa=leg_angle[0].sol_alfa/M_PI*180;
	//  leg_angle[0].sol_beta=leg_angle[0].sol_beta/M_PI*180;

	// 左前腿：************************************************************************************************************************************
	//  计算gama角
	leg_angle[1].sol_gamma = acos(leg_length1 / (sqrt(foot_position[1].y * foot_position[1].y + foot_position[1].z * foot_position[1].z))) - atan((-foot_position[1].z) / foot_position[1].y);
	// 计算a1角
	leg_angle[1].a1 = atan(foot_position[1].x / (foot_position[1].z + leg_length1 * sin(leg_angle[1].sol_gamma)));
	// 计算CE长度
	leg_angle[1].l4 = sqrt(foot_position[1].x * foot_position[1].x + (foot_position[1].y - leg_length1 * cos(leg_angle[1].sol_gamma)) * (foot_position[1].y - leg_length1 * cos(leg_angle[1].sol_gamma)) + (foot_position[1].z + leg_length1 * sin(leg_angle[1].sol_gamma)) * (foot_position[1].z + leg_length1 * sin(leg_angle[1].sol_gamma)));
	// 计算a角
	leg_angle[1].sol_alfa = (acos((leg_length2 * leg_length2 + leg_angle[1].l4 * leg_angle[1].l4 - leg_length3 * leg_length3) / (2 * leg_length2 * leg_angle[1].l4)) - leg_angle[1].a1);
	// 计算b角
	leg_angle[1].sol_beta = (acos((leg_length2 * leg_length2 + leg_length3 * leg_length3 - leg_angle[1].l4 * leg_angle[1].l4) / (2 * leg_length2 * leg_length3)) - M_PI / 2);
	// //转换成角度制
	// leg_angle[1].sol_gamma=leg_angle[1].sol_gamma/M_PI*180;
	// leg_angle[1].sol_alfa=leg_angle[1].sol_alfa/M_PI*180;
	// leg_angle[1].sol_beta=leg_angle[1].sol_beta/M_PI*180;

	// 右后腿：************************************************************************************************************************************
	//  计算gama角
	leg_angle[2].sol_gamma = acos(leg_length1 / (sqrt(foot_position[2].y * foot_position[2].y + foot_position[2].z * foot_position[2].z))) - atan(foot_position[2].z / foot_position[2].y);
	// 计算a1角
	leg_angle[2].a1 = atan(foot_position[2].x / (foot_position[2].z + leg_length1 * sin(leg_angle[2].sol_gamma)));
	// 计算CE长度
	leg_angle[2].l4 = sqrt(foot_position[2].x * foot_position[2].x + (foot_position[2].y - leg_length1 * cos(leg_angle[2].sol_gamma)) * (foot_position[2].y - leg_length1 * cos(leg_angle[2].sol_gamma)) + (foot_position[2].z + leg_length1 * sin(leg_angle[2].sol_gamma)) * (foot_position[2].z + leg_length1 * sin(leg_angle[2].sol_gamma)));
	// 计算a角
	leg_angle[2].sol_alfa = (acos((leg_length2 * leg_length2 + leg_angle[2].l4 * leg_angle[2].l4 - leg_length3 * leg_length3) / (2 * leg_length2 * leg_angle[2].l4)) - leg_angle[2].a1);
	// 计算b角
	leg_angle[2].sol_beta = (acos((leg_length2 * leg_length2 + leg_length3 * leg_length3 - leg_angle[2].l4 * leg_angle[2].l4) / (2 * leg_length2 * leg_length3)) - M_PI / 2);
	// 转换成角度制
	//  leg_angle[2].sol_gamma=leg_angle[2].sol_gamma/M_PI*180;
	//  leg_angle[2].sol_alfa=leg_angle[2].sol_alfa/M_PI*180;
	//  leg_angle[2].sol_beta=leg_angle[2].sol_beta/M_PI*180;

	// 左后腿:************************************************************************************************************************************
	//  计算gama角
	leg_angle[3].sol_gamma = acos(leg_length1 / (sqrt(foot_position[3].y * foot_position[3].y + foot_position[3].z * foot_position[3].z))) - atan((-foot_position[3].z) / foot_position[3].y);
	// 计算a1角
	leg_angle[3].a1 = atan(foot_position[3].x / (foot_position[3].z + leg_length1 * sin(leg_angle[3].sol_gamma)));
	// 计算CE长度
	leg_angle[3].l4 = sqrt(foot_position[3].x * foot_position[3].x + (foot_position[3].y - leg_length1 * cos(leg_angle[3].sol_gamma)) * (foot_position[3].y - leg_length1 * cos(leg_angle[3].sol_gamma)) + (foot_position[3].z + leg_length1 * sin(leg_angle[3].sol_gamma)) * (foot_position[3].z + leg_length1 * sin(leg_angle[3].sol_gamma)));
	// 计算a角
	leg_angle[3].sol_alfa = (acos((leg_length2 * leg_length2 + leg_angle[3].l4 * leg_angle[3].l4 - leg_length3 * leg_length3) / (2 * leg_length2 * leg_angle[3].l4)) - leg_angle[3].a1);
	// 计算b角
	leg_angle[3].sol_beta = (acos((leg_length2 * leg_length2 + leg_length3 * leg_length3 - leg_angle[3].l4 * leg_angle[3].l4) / (2 * leg_length2 * leg_length3)) - M_PI / 2);
	// 转换成角度制
	//  leg_angle[3].sol_gamma=leg_angle[3].sol_gamma/M_PI*180;
	//  leg_angle[3].sol_alfa=leg_angle[3].sol_alfa/M_PI*180;
	//  leg_angle[3].sol_beta=leg_angle[3].sol_beta/M_PI*180;
	// int yu[12];






















	// double output_bias[4][3] = {{0.99,-1.59,0},{0.99,-0.159,0},{0.0.599,-1.59,0},{0.99,-1.59,0}};
	double output_bias[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	double motor_bias_lying_to_stand[12] = {0.4, -0.4, 0.4, -0.4, -0.99, -0.99, -0.99, -0.99, 1.59, 1.59, 1.59, 1.59};
	double motor_bias_stand_to_kneel[12] = {0, 0, 0, 0, 0.5, 0.5, 0.5, 0.5, 0.3, 0.3, 0.3, 0.3};

	double ksmaller_alfa = 0.4;
	double ksmaller_beta = 2.0;
	double ksmaller_gamma = 0.4;

	double bias_alfa = -0.7;
	double bias_beta = 2.0;
	double bias_gamma = 0.0;

	// leg_angle_ouput[i].sol_alfa = -leg_angle[i].sol_alfa * ksmaller + bias_alfa + (0.1) - (i == 2 ? 0.2 : 0.0);
	// leg_angle_ouput[i].sol_beta = -leg_angle[i].sol_beta * ksmaller + bias_beta + (i == 2 ? 0.1 : 0.0);

	leg_angle_ouput[0].sol_alfa = leg_angle[0].sol_alfa * ksmaller_alfa + bias_alfa - 0.2; // 右前腿
	leg_angle_ouput[0].sol_beta = leg_angle[0].sol_beta * ksmaller_beta + bias_beta ;
	leg_angle_ouput[0].sol_gamma = leg_angle[0].sol_gamma * ksmaller_gamma * 0.5+ bias_gamma;

	leg_angle_ouput[1].sol_alfa = leg_angle[3].sol_alfa * ksmaller_alfa + bias_alfa - 0.2; // 左前腿
	leg_angle_ouput[1].sol_beta = leg_angle[3].sol_beta * ksmaller_beta + bias_beta ;
	leg_angle_ouput[1].sol_gamma = leg_angle[1].sol_gamma * ksmaller_gamma * 0.5+ bias_gamma;

	leg_angle_ouput[2].sol_alfa = leg_angle[0].sol_alfa * ksmaller_alfa + bias_alfa - 0.2; // 左后腿
	leg_angle_ouput[2].sol_beta = leg_angle[0].sol_beta * ksmaller_beta + bias_beta;
	leg_angle_ouput[2].sol_gamma = leg_angle[2].sol_gamma * ksmaller_gamma * 0.5+ bias_gamma;

	leg_angle_ouput[3].sol_alfa = leg_angle[3].sol_alfa * ksmaller_alfa + bias_alfa - 0.2; // 右后腿
	leg_angle_ouput[3].sol_beta = leg_angle[3].sol_beta * ksmaller_beta + bias_beta;
	leg_angle_ouput[3].sol_gamma = leg_angle[3].sol_gamma * ksmaller_gamma * 0.5 + bias_gamma;
}
